#include "ti_msp_dl_config.h"

#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"

#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>

#include "mpu6050.h"
#include "mspm0_i2c.h"

/* Data requested by client. */
#define PRINT_ACCEL     (0x01)
#define PRINT_GYRO      (0x02)
#define PRINT_QUAT      (0x04)

#define ACCEL_ON        (0x01)
#define GYRO_ON         (0x02)

#define MOTION          (0)
#define NO_MOTION       (1)

/* Starting sampling rate. */
#define DEFAULT_MPU_HZ  (50)

#define FLASH_SIZE      (512)
#define FLASH_MEM_START ((void*)0x1800)

struct rx_s {
    unsigned char header[3];
    unsigned char cmd;
};
struct hal_s {
    unsigned char sensors;
    unsigned char dmp_on;
    unsigned char wait_for_tap;
    volatile unsigned char new_gyro;
    unsigned short report;
    unsigned short dmp_features;
    unsigned char motion_int_mode;
    struct rx_s rx;
};
static struct hal_s hal = {0};

unsigned long sensor_timestamp;
short gyro[3], accel[3], sensors;
unsigned char more;
long quat[4];

#define q30  (1073741824.0f) /* 2^30 = 1073741824 */
float pitch, roll, yaw;

/* 传感器可以以任意方向安装到电路板上。
 * 下面的安装矩阵（mounting matrix）用于告诉 MPL（Motion Processing Library）
 * 如何旋转来自驱动程序的原始数据。
 * 
 * TODO: 以下矩阵是 Invensense 内部测试板的默认配置。
 *       如果有需要，请根据你的传感器实际安装方向，
 *       修改这些矩阵以匹配芯片到机体的方向关系（chip-to-body matrix）。
 */
static signed char gyro_orientation[9] = {-1, 0, 0,
                                           0,-1, 0,
                                           0, 0, 1};

static void tap_cb(unsigned char direction, unsigned char count)
{

}

static void android_orient_cb(unsigned char orientation)
{

}

/* 接下来的两个函数将方向矩阵（参考 gyro_orientation）
 * 转换为 DMP 使用的标量形式。
 * 注意：这些函数来自于 Invensense 的 MPL（Motion Processing Library）。
 */
static inline unsigned short inv_row_2_scale(const signed char *row)
{
    unsigned short b;

    if (row[0] > 0)
        b = 0;
    else if (row[0] < 0)
        b = 4;
    else if (row[1] > 0)
        b = 1;
    else if (row[1] < 0)
        b = 5;
    else if (row[2] > 0)
        b = 2;
    else if (row[2] < 0)
        b = 6;
    else
        b = 7;      // error
    return b;
}

static inline unsigned short inv_orientation_matrix_to_scalar(
    const signed char *mtx)
{
    unsigned short scalar;

    /*
       XYZ  010_001_000 Identity Matrix
       XZY  001_010_000
       YXZ  010_000_001
       YZX  000_010_001
       ZXY  001_000_010
       ZYX  000_001_010
     */

    scalar = inv_row_2_scale(mtx);
    scalar |= inv_row_2_scale(mtx + 3) << 3;
    scalar |= inv_row_2_scale(mtx + 6) << 6;


    return scalar;
}

void MPU6050_Init(void)
{
    int result;
    unsigned char accel_fsr;                // 加速度计满量程范围（±2g/4g/8g/16g）
    unsigned short gyro_rate, gyro_fsr;     // 陀螺仪采样率和满量程范围（±250/500/1000/2000°/s）

    // 检查 I2C SDA 状态，如果被锁死（SDA 低电平挂死），执行解锁处理
    if(DL_I2C_getSDAStatus(I2C_MPU6050_INST) == DL_I2C_CONTROLLER_SDA_LOW)
        mpu6050_i2c_sda_unlock();

    // 初始化 MPU6050
    result = mpu_init();
    if (result)
        DL_SYSCTL_resetDevice(DL_SYSCTL_RESET_POR); // 初始化失败，重启系统（上电复位）

    result = 0;

    /* 配置传感器 */
    // 使能陀螺仪和加速度计
    result += mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);

    // 配置 FIFO 缓冲区，把陀螺仪和加速度计数据压入 FIFO
    result += mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);

    // 设置采样率（一般是 100Hz、200Hz 等）
    result += mpu_set_sample_rate(DEFAULT_MPU_HZ);

    /* 读取当前采样率和满量程设置，验证配置是否成功 */
    result += mpu_get_sample_rate(&gyro_rate);
    result += mpu_get_gyro_fsr(&gyro_fsr);
    result += mpu_get_accel_fsr(&accel_fsr);

    /* 初始化 HAL（硬件抽象层）状态变量 */
    memset(&hal, 0, sizeof(hal));       // 清零 HAL 结构体
    hal.sensors = ACCEL_ON | GYRO_ON;   // 表示当前使能了加速度计和陀螺仪
    hal.report = PRINT_QUAT;            // 设置数据输出格式为四元数（常用于姿态）

    /* 初始化 DMP（数字运动处理器）步骤如下：
     * 1. 调用 dmp_load_motion_driver_firmware()，该函数会将
     *    inv_mpu_dmp_motion_driver.h 中的 DMP 固件镜像写入 MPU 内部内存。
     * 2. 将陀螺仪和加速度计的方向矩阵加载到 DMP 中。
     * 3. 注册手势回调函数。注意，这些回调函数只有在启用了相应功能后才会触发。
     * 4. 调用 dmp_enable_feature(mask) 启用所需的 DMP 功能。
     * 5. 调用 dmp_set_fifo_rate(freq) 设置 DMP 的输出速率。
     * 6. 根据需要调用其他特定功能的控制函数。
     *
     * 要启用 DMP，只需调用 mpu_set_dmp_state(1) 即可。
     * 此函数可重复调用，用于运行时启用或禁用 DMP。
     *
     * 以下是 inv_mpu_dmp_motion_driver.c 中提供的 DMP 固件支持的功能简要说明：
     * DMP_FEATURE_LP_QUAT：在 DMP 内部以 200Hz(5ms) 的速率生成仅基于陀螺仪的四元数,这种高频积分方式能降低数值误差（相比 MCU 低频处理方式更精确）。
     * DMP_FEATURE_6X_LP_QUAT：在 DMP 内部以 200Hz 的速率生成 陀螺仪 + 加速度计 融合的四元数,不能与 DMP_FEATURE_LP_QUAT 同时使用。
     * DMP_FEATURE_TAP：检测 X、Y、Z 轴方向上的敲击（tap）事件。
     * DMP_FEATURE_ANDROID_ORIENT：谷歌 Android 的屏幕旋转算法,在设备达到四种屏幕旋转方向时触发事件。
     * DMP_FEATURE_GYRO_CAL：在设备静止八秒后自动对陀螺仪数据进行校准。
     * DMP_FEATURE_SEND_RAW_ACCEL：将原始加速度计数据写入 FIFO。
     * DMP_FEATURE_SEND_RAW_GYRO：将原始陀螺仪数据写入 FIFO。
     * DMP_FEATURE_SEND_CAL_GYRO：将校准后的陀螺仪数据写入 FIFO,不能与 DMP_FEATURE_SEND_RAW_GYRO 同时使用。
     * 
     */

    result += dmp_load_motion_driver_firmware();
    result += dmp_set_orientation(
        inv_orientation_matrix_to_scalar(gyro_orientation));
    result += dmp_register_tap_cb(tap_cb);
    result += dmp_register_android_orient_cb(android_orient_cb);
    hal.dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
        DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
        DMP_FEATURE_GYRO_CAL;
    result += dmp_enable_feature(hal.dmp_features);
    result += dmp_set_fifo_rate(DEFAULT_MPU_HZ);
    result += mpu_set_dmp_state(1);
    hal.dmp_on = 1;

    if (result)
        DL_SYSCTL_resetDevice(DL_SYSCTL_RESET_POR);

    //NVIC_EnableIRQ(GPIO_MPU6050_INT_IRQN);
}

int Read_Quad(void)
{
    /* 当启用了 DMP（数字运动处理器）时，此函数用于从 FIFO 中读取新数据。
     * FIFO 中可能包含任意组合的陀螺仪、加速度计、四元数和手势识别数据。
     *
     * 参数 sensors 告诉调用者哪些传感器的数据字段被填充了新数据。
     * 例如，如果 sensors == (INV_XYZ_GYRO | INV_WXYZ_QUAT)，
     * 表示 FIFO 中没有包含加速度计的数据。
     *
     * 驱动程序会自动解析手势数据，以判断是否发生了手势事件；
     * 如果发生了手势事件，且应用程序注册了相应的回调函数，
     * 则会通过回调通知应用程序。
     *
     * 参数 more 如果非零，表示 FIFO 中还有剩余的数据包尚未处理。
     */


    int result;

    do
    {
        result = dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more);
    }while(more);

    if(result)
        return -1;

    float q0 = quat[0] / q30;
    float q1 = quat[1] / q30;
    float q2 = quat[2] / q30;
    float q3 = quat[3] / q30;

    pitch  = asin(-2 * q1 * q3 + 2 * q0 * q2) * 57.3;
    roll   = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 57.3;
    yaw    = atan2(2 * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * 57.3;

    return 0;
}